
//#include <iostream>
#include <iostream>
#include <string.h>
#include "ros/ros.h"
#include "SerialStream.h"
#include "robot_msgs/RobotPosition.h"
#include "kalman.h"


/**
* 显示数据
*/
void Display(char buf[], int ibuf[], double x, double y)
{
    
    printf("[%d %3d %3d %3d %3d %d %d %d %d %d %d]-->"
        , buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]
        , buf[6], buf[7], buf[8], buf[9], buf[10]);
    printf("[%d %d %d %d %d %d]-->"
        , ibuf[0], ibuf[1], ibuf[2], ibuf[3], ibuf[4], ibuf[5]);
    
    printf("%f %f\n", x, y);
}


/**
* 解析buf的内容并计算生成位置信息。
*/
bool compute_pos(char buf_0[], char buf_1[], char buf_2[], int &x, int &y)
{
    static int i=0;

    x = 0.2*i;
    y = 0.3*i;
    i++;
    return true;
}

/**
* 从串口读取指定长度的数据。由于一些未知原因，无法使用read函数直接读取指定
* 长度的数据，所以有了这个函数。另外，阻塞读的方式会由于中断打断而退出，而
* 这种工作方式，不会受中断影响。
*
*/
void ReadSerial( LibSerial::SerialStream &il_serial, char buf[], int len)
{
    for(int i=0; i<len; i++){
        il_serial.read( buf+i, 1);
    }
}


/**
* Init serial port for indor-localization model.
*/
void InitSerialPort(LibSerial::SerialStream &il_serial)
{
   /**
    * The various available baud rates are defined in SerialStreamBuf 
    * class. This is to be changed soon. All serial port settings will 
    * be placed in in the SerialPort class.
    */
    il_serial.SetBaudRate( LibSerial::SerialStreamBuf::BAUD_115200 ) ;
    
   /**
    * Use 8 bit wide characters.
    */
    il_serial.SetCharSize( LibSerial::SerialStreamBuf::CHAR_SIZE_8 ) ;
    
   /**
    * Use one stop bit.
    */
    il_serial.SetNumOfStopBits(1) ;   
    
   /**
    * Use odd parity during serial communication.
    */
    il_serial.SetParity( LibSerial::SerialStreamBuf::PARITY_NONE ) ;

   /**
    * Use hardware flow-control.
    */
    il_serial.SetFlowControl( LibSerial::SerialStreamBuf::FLOW_CONTROL_NONE ) ;
    
}

/**
* 读取单个帧数据。
*/
bool ReadSingleFrame(LibSerial::SerialStream &il_serial, char frame_header[3], char buf[], int len){
    while(1){
        char data[3];
        il_serial.read( data, 1);
        if(data[0]==(char)frame_header[0]){
            il_serial.read( data+1, 1);
            if(data[1]==(char)frame_header[1]){
              il_serial.read( data+2, 1);
              if(data[2]==(char)frame_header[2]){
                buf[0]=frame_header[0];
                buf[1]=frame_header[1];
                buf[2]=frame_header[2];
                break;
              }
            }
            memset(data, 0, sizeof(data));
        }
    }
    ReadSerial(il_serial, buf+3, len-3);
    return true;
}


/**
* 从室内定位模块的串口读取数据，并存入三个buf中。
*/
bool ReadPosSensor(LibSerial::SerialStream &il_serial , char buf_0[11]
    , char buf_1[11], char buf_2[11], int len=11)
{
    // Find Frame Based on Frame Header

    char frame_header_0[]={0xe0, 0x00, 0x01};
    char frame_header_1[]={0xe0, 0x01, 0x01};
    char frame_header_2[]={0xe0, 0x02, 0x01};
    if(ReadSingleFrame(il_serial,frame_header_0, buf_0, len)
      &&ReadSingleFrame(il_serial,frame_header_1, buf_1, len)
      &&ReadSingleFrame(il_serial,frame_header_2, buf_2, len)){
        return true;
    }

    return false;
}

int main(int argc, char **argv)
{
   /* Init Node.*/
    ros::init(argc, argv, "indoor_localization_node");
    ros::NodeHandle n;

   /* Create and init the serial port for indoor-localization.*/
    //LibSerial::SerialStream il_serial("/dev/ttyUSB0",std::ios_base::in);
    LibSerial::SerialStream il_serial("/dev/ttyUSB0",std::ios_base::in);
    InitSerialPort(il_serial);
    ros::Publisher pos_pub = n.advertise<robot_msgs::RobotPosition>("robot/sensor/position", 50);


    while(ros::ok()){
        /** Receive data from serial port. */
        const int kOriginBufSize = 11 ;
        char buf_0[kOriginBufSize];
        char buf_1[kOriginBufSize];
        char buf_2[kOriginBufSize];
        if(ReadPosSensor(il_serial, buf_0, buf_1, buf_2, kOriginBufSize)){
            double x, y;
            const int kBufSize = 6;
            int ibuf_0[kBufSize]={0};
            int ibuf_1[kBufSize]={0};
            int ibuf_2[kBufSize]={0};
            // Computing position
            DataTransform(buf_0, ibuf_0, kOriginBufSize);
            DataTransform(buf_1, ibuf_1, kOriginBufSize);
            DataTransform(buf_2, ibuf_2, kOriginBufSize);
            
            position(ibuf_0, ibuf_1, ibuf_2, x, y);
            Display( buf_0, ibuf_0, x, y);
            Display( buf_1, ibuf_1, x, y);
            Display( buf_2, ibuf_2, x, y);
            //if(!compute_pos(buf_0, buf_1, buf_2, x, y)){
            //    std::cerr<<"Error: "<<std::endl;
            //    continue;
            //}

            // Publish to position topic
            robot_msgs::RobotPosition pos_msg;
            pos_msg.sensor_id = 0;
            pos_msg.x = x/100;
            pos_msg.y = y/100;
            pos_msg.z = 0;
            pos_pub.publish(pos_msg);
        }

    }

    il_serial.Close() ;
    
    return 0;
}


